/**********************************************************
**             Email:@qq.com   QQ:1069841355
**---------------------------------------------------------
**  Description: 此文件为 四足机器人 控制器 文件
**  Version    : 
**  Notes      :
**  Author     : 于宪元
**********************************************************/
#include <stdbool.h>
#include <stdio.h>
#include <math.h>
#include <stdlib.h>

#include <webots/keyboard.h>

#include "easyMat.h"
#include "controller.h"

robotTypeDef robot;
//----------------------------------------------------------declaration
void update_vxd_wzd();
void update_IMU();
void update_foot_touch_sensor();
void phase_swap();
void update_theta();
void forwardKinematics();
void estimate_vx_vy_wz();
void create_invQ(matTypeDef* invQ, vect3TypeDef stF_toe, vect3TypeDef stB_toe);
void create_transJ(matTypeDef* transJ, legNameTypeDef Fleg, legNameTypeDef Bleg);
void curve(vect3TypeDef* ft, vect3TypeDef* dft, double xT, double yT, vect3TypeDef toe0, vect3TypeDef dtoe0);
void update_toe0_dtoe0();
void update_toe_err_sum();
/*
机器人参数初始化，在while（1）之前调用
*/
void robot_init()
{
  //------------------------------------------------------------------------控制参数
   robot.Tf     = 0.3;
   robot.Ts     = 0.3;
   robot.hd     = 0.65;
   robot.zh     = -0.45;
   robot.Kr     = 2000;
   robot.Kdr    = 120;
   robot.Kp     = 10000;
   robot.Kdp    = 800;
   robot.Kh     = 8000;
   robot.Kdh    = 800;
   robot.Kvx    = 1000;
   robot.Kwz    = 1000;
   robot.Kvy    = 0.02;
   robot.kx     = 100;
   robot.kdx    = 100;
   robot.ky     = 8000;
   robot.kdy    = 100;
   robot.kz1    = 10000;
   robot.kz2    = 1000;
   robot.kdz    = 100;
   robot.tes[4] = 0;    //toe_err_sum
   robot.I      = 0.005;

   robot.vxd_default = 0.8;  //默认前向期望速度
   robot.wzd_default = 15;   //默认转弯速度
   robot.vxd = 0;
   robot.vyd = 0;
   robot.wzd = 0;
  //------------------------------------------------------------------------模型参数
   robot.l  = 0.45;   //前后横滚髋关节距离的一半
   robot.w  = 0.175;  //左右横滚髋关节距离的一半
   robot.h  = 0.1;    //躯干质心与横滚髋关节垂直距离
   robot.a0 = 0.05;   //髋部连杆长度
   robot.a1 = 0.3;    //大腿连杆长度
   robot.a2 = 0.3;    //小腿连杆长度
   robot.m0 = 2;      //髋部连杆质量
   robot.m1 = 4;      //大腿连杆质量
   robot.m2 = 4;      //小腿连杆质量
   robot.M  = 100;    //躯干质量
  //------------------------------------------------------------------------状态区
   robot.t               = 0;      //从一步开始，经过的时间
   robot.Fst             = FL;     //前支撑腿
   robot.Bst             = BR;     //后支撑腿
   robot.Fsw             = FR;     //前摆动腿
   robot.Bsw             = BL;     //后摆动腿
   robot.is_touching[FL] = false;  //足底传感器接触标志量
   robot.is_touching[FR] = false;  //足底传感器接触标志量
   robot.is_touching[BL] = false;  //足底传感器接触标志量
   robot.is_touching[BR] = false;  //足底传感器接触标志量
   robot.R               = 0;      //roll
   robot.P               = 0;      //pitch
   robot.Y               = 0;      //yaw
   robot.dR              = 0;      //roll'
   robot.dP              = 0;      //pitch'
   robot.dY              = 0;      //yaw'
   for(legNameTypeDef legIdx = FL; legIdx <= BR; legIdx++)
   {
     robot.dtoe[legIdx].x = 0;
     robot.dtoe[legIdx].y = 0;
     robot.dtoe[legIdx].z = 0;
   }
   robot.vx              = 0;  //机器人实际x速度
   robot.vy              = 0;  //机器人实际y速度
   robot.wz              = 0;  //机器人实际z速度
   robot.Fdtoe0.x        = 0;
   robot.Fdtoe0.y        = 0;
   robot.Fdtoe0.z        = 0;
   robot.Bdtoe0.x        = 0;
   robot.Bdtoe0.y        = 0;
   robot.Bdtoe0.z        = 0;
}
/*
机器人状态更新，包括读取传感器的数据以及一些状态估计
*/
void updateRobotState()
{
  /*通过键盘修改期望速度*/
  update_vxd_wzd();
  /*IMU和IMU导数更新*/
  update_IMU();
  /*足底接触传感器更新*/
  update_foot_touch_sensor();
  /*相位切换*/
  phase_swap();
  /*更新关节角*/
  update_theta();
  /*运动学正解*/
  forwardKinematics();
  /*记录相位切换瞬间，摆动足的起始位置和起始速度*/
  update_toe0_dtoe0();
  /*机器人水平速度估计*/
  estimate_vx_vy_wz();  //在robot.t=0时候特殊处理
  /*更新足底横向误差*/
  update_toe_err_sum();
  /*时钟更新*/
  robot.t += 0.001*TIME_STEP;   
}
/*
机器人控制
*/
void robot_control()
{
  /*摔倒时，停止控制*/
  static bool fall_flag = false;
  if(fabs(robot.R) >= 30) fall_flag = true;
  if(fall_flag == true)
  {
   all_motor_stop();
   return;
  }
  //分别提取支撑足与摆动足的足底坐标
  vect3TypeDef stF_toe, stB_toe, swF_toe, swB_toe;
  if(robot.Fst == FL)
  {
    stF_toe = robot.toe[FL];
    stB_toe = robot.toe[BR];
    swF_toe = robot.toe[FR];
    swB_toe = robot.toe[BL];
  }
  else  //(robot.Fst == FR)
  {
    stF_toe = robot.toe[FR];
    stB_toe = robot.toe[BL];
    swF_toe = robot.toe[FL];
    swB_toe = robot.toe[BR];
  }
  //-----------------------------------------------------------------支撑相控制
  //求伪俯仰角
  static double pre_psi = 0;
  double psi  = atan((stF_toe.z - stB_toe.z)/(stF_toe.x - stB_toe.x))*180.0/PI;
  double dpsi = (psi - pre_psi)/(0.001*(double)TIME_STEP);
  pre_psi = psi;
  //求机身高度
  static double pre_h = 0.6;
  double h  = -(stF_toe.z + stB_toe.z)/2.0;
  double dh = (h - pre_h)/(0.001*(double)TIME_STEP);
  pre_h = h;
  //求机身六维力(取消对Fy的控制)
  double Tx = -(robot.Kr*robot.R/180.0*PI + robot.Kdr*robot.dR/180.0*PI);
  double Ty = -(robot.Kp*psi/2.0/180.0*PI + robot.Kdp*dpsi/2.0/180.0*PI);
  double Fz = -(robot.Kh*(h - robot.hd) + robot.Kdh*dh);
  double Fx = -(robot.Kvx*(robot.vx - robot.vxd));
  double Tz = -(robot.Kwz*(robot.wz - robot.wzd)/180.0*PI);
  //求支撑足足底力
  matTypeDef st_f, invQ, F;
  easyMat_create(&st_f, 6, 1);
  easyMat_create(&F   , 6, 1);
  easyMat_create(&invQ, 6, 6);
  F.data[0][0] = Fx - robot.M*g*sin(robot.P/180.0*PI) + 55;//55是偏置量，防止踏步向后移动
  F.data[1][0] = Fz + robot.M*g*cos(robot.P/180.0*PI);
  F.data[2][0] = Tx;
  F.data[3][0] = Ty;
  F.data[4][0] = Tz;
  F.data[5][0] = 0 ;
  create_invQ(&invQ, stF_toe, stB_toe);
  easyMat_mult(&st_f, &invQ, &F);
  //求支撑足关节力
  matTypeDef st_transJ, st_tau;
  easyMat_create(&st_tau   , 6, 1);
  easyMat_create(&st_transJ, 6, 6);
  create_transJ(&st_transJ, robot.Fst, robot.Bst);
  easyMat_mult(&st_tau, &st_transJ, &st_f);
  easyMat_mult_k(-1.0, &st_tau);
  //发送关节力到电机
  if(robot.Fst == FL)
  {
    set_motor_torque(FL0, st_tau.data[0][0]);
    set_motor_torque(FL1, st_tau.data[1][0]);
    set_motor_torque(FL2, st_tau.data[2][0]);
    set_motor_torque(BR0, st_tau.data[3][0]);
    set_motor_torque(BR1, st_tau.data[4][0]);
    set_motor_torque(BR2, st_tau.data[5][0]);
  }
  else //(robot.Fst == FR)
  {
    set_motor_torque(FR0, st_tau.data[0][0]);
    set_motor_torque(FR1, st_tau.data[1][0]);
    set_motor_torque(FR2, st_tau.data[2][0]);
    set_motor_torque(BL0, st_tau.data[3][0]);
    set_motor_torque(BL1, st_tau.data[4][0]);
    set_motor_torque(BL2, st_tau.data[5][0]);
  }
  //释放内存
  easyMat_free(&st_f     );
  easyMat_free(&invQ     );
  easyMat_free(&F        );
  easyMat_free(&st_transJ);
  easyMat_free(&st_tau   );
  //-----------------------------------------------------------------摆动相控制
  /*注意，摆动相控制，足底轨迹与坐标都是在单腿坐标系下规划，而不是机体坐标系*/
  //-----------------------------------------------------------------摆动相控制
  //将支撑足底坐标从机体坐标系转移到单腿坐标系下
  if(robot.Fsw == FL)
  {
    swF_toe.x = swF_toe.x - robot.l;
    swF_toe.y = swF_toe.y - robot.w;  //FR
    swB_toe.x = swB_toe.x + robot.l;
    swB_toe.y = swB_toe.y + robot.w;  //BL
  }
  else//(robot.Fsw == FR)
  {
    swF_toe.x = swF_toe.x - robot.l;
    swF_toe.y = swF_toe.y + robot.w;  //FL
    swB_toe.x = swB_toe.x + robot.l;
    swB_toe.y = swB_toe.y - robot.w;  //BR
  }
  //求落足点
  double xT = robot.vxd*robot.Ts/2.0;
  double yT = robot.vy*robot.Ts/2.0 + 
              robot.Kvy*(robot.vy - robot.vyd) + 
              robot.I*(robot.tes[robot.Fsw] + robot.tes[robot.Bsw]);
  //求摆动足实时期望足底坐标与速度
  static vect3TypeDef pre_Ftoed, pre_Btoed;
  vect3TypeDef Ftoed, Btoed, Fdtoed, Bdtoed;
  curve(&Ftoed, &Fdtoed, xT, yT, robot.Ftoe0, robot.Fdtoe0);
  curve(&Btoed, &Bdtoed, xT, yT, robot.Btoe0, robot.Bdtoe0);
  if(robot.t > 0.75*robot.Tf)  //时间过去3/4，三足着地时，着地的摆动足保持位置不变
  {
    if(robot.is_touching[robot.Fsw]) 
    {
      Ftoed  = pre_Ftoed;
      Fdtoed.x = 0;
      Fdtoed.y = 0;
      Fdtoed.z = 0;
    }
    if(robot.is_touching[robot.Bsw])
    {
      Btoed  = pre_Btoed;
      Bdtoed.x = 0;
      Bdtoed.y = 0;
      Bdtoed.z = 0;
    }
  }
  pre_Ftoed = Ftoed;
  pre_Btoed = Btoed;
  //求足底力
  matTypeDef sw_f;
  easyMat_create(&sw_f, 6,1);
  
  double kz;
  if(robot.t <= 0.75*robot.Tf)
    kz = robot.kz1;
  else
    kz = robot.kz2;
  
  sw_f.data[0][0] = -(robot.kx*(swF_toe.x - Ftoed.x) + robot.kdx*(robot.dtoe[robot.Fsw].x - Fdtoed.x));
  sw_f.data[1][0] = -(robot.ky*(swF_toe.y - Ftoed.y) + robot.kdy*(robot.dtoe[robot.Fsw].y - Fdtoed.y));
  sw_f.data[2][0] = -(      kz*(swF_toe.z - Ftoed.z) + robot.kdz*(robot.dtoe[robot.Fsw].z - Fdtoed.z));
  sw_f.data[3][0] = -(robot.kx*(swB_toe.x - Btoed.x) + robot.kdx*(robot.dtoe[robot.Bsw].x - Bdtoed.x));
  sw_f.data[4][0] = -(robot.ky*(swB_toe.y - Btoed.y) + robot.kdy*(robot.dtoe[robot.Bsw].y - Bdtoed.y));
  sw_f.data[5][0] = -(      kz*(swB_toe.z - Btoed.z) + robot.kdz*(robot.dtoe[robot.Bsw].z - Bdtoed.z));
  //求关节力
  matTypeDef sw_transJ, sw_tau;
  easyMat_create(&sw_tau   , 6, 1);
  easyMat_create(&sw_transJ, 6, 6);
  create_transJ(&sw_transJ, robot.Fsw, robot.Bsw);
  easyMat_mult(&sw_tau, &sw_transJ, &sw_f);
  //发送关节力到电机
  if(robot.Fsw == FL)
  {
    set_motor_torque(FL0, sw_tau.data[0][0]);
    set_motor_torque(FL1, sw_tau.data[1][0]);
    set_motor_torque(FL2, sw_tau.data[2][0]);
    set_motor_torque(BR0, sw_tau.data[3][0]);
    set_motor_torque(BR1, sw_tau.data[4][0]);
    set_motor_torque(BR2, sw_tau.data[5][0]);
  }
  else if(robot.Fsw == FR)
  {
    set_motor_torque(FR0, sw_tau.data[0][0]);
    set_motor_torque(FR1, sw_tau.data[1][0]);
    set_motor_torque(FR2, sw_tau.data[2][0]);
    set_motor_torque(BL0, sw_tau.data[3][0]);
    set_motor_torque(BL1, sw_tau.data[4][0]);
    set_motor_torque(BL2, sw_tau.data[5][0]);
  }
  //释放内存
  easyMat_free(&sw_f     );
  easyMat_free(&sw_transJ);
  easyMat_free(&sw_tau   );
}
/*
通过键盘修改期望速度
*/
void update_vxd_wzd()
{
   /*读取键盘，获取速度*/
  switch(get_keyboard())
  {
    case WB_KEYBOARD_UP:
    {
      robot.vxd = robot.vxd_default;
      break;
    }
    case WB_KEYBOARD_DOWN:
    {
      robot.vxd = -robot.vxd_default;
      break;
    }
    case WB_KEYBOARD_RIGHT:
    {
      robot.wzd = -robot.wzd_default;
      break;
    }
    case WB_KEYBOARD_LEFT:
    {
      robot.wzd = robot.wzd_default;
      break;
    }
    default:
    {
      robot.vxd = 0;
      robot.wzd = 0;
      break;
    }
  }
}
/*
足底传感器更新
*/
void update_foot_touch_sensor()
{
  robot.is_touching[FL] = is_foot_touching(FL);
  robot.is_touching[FR] = is_foot_touching(FR);
  robot.is_touching[BL] = is_foot_touching(BL);
  robot.is_touching[BR] = is_foot_touching(BR);
}
/*
IMU与IMU的导数更新
*/
void update_IMU()
{
  static eulerAngleTypeDef pre_eulerAngle = {0,0,0};
  eulerAngleTypeDef eulerAngle = get_IMU_Angle();
  //IMU
  robot.R  = eulerAngle.roll;
  robot.P  = eulerAngle.pitch;
  robot.Y  = eulerAngle.yaw;
  //IMU'
  robot.dR = (eulerAngle.roll  - pre_eulerAngle.roll )/((double)TIME_STEP/1000.0);
  robot.dP = (eulerAngle.pitch - pre_eulerAngle.pitch)/((double)TIME_STEP/1000.0);
  robot.dY = (eulerAngle.yaw   - pre_eulerAngle.yaw  )/((double)TIME_STEP/1000.0);
  
  pre_eulerAngle = eulerAngle;
}
/*
更新关节角
*/
void update_theta()
{
  robot.theta[FL][0] = get_motor_angle(FL0);
  robot.theta[FL][1] = get_motor_angle(FL1);
  robot.theta[FL][2] = get_motor_angle(FL2);
  
  robot.theta[FR][0] = get_motor_angle(FR0);
  robot.theta[FR][1] = get_motor_angle(FR1);
  robot.theta[FR][2] = get_motor_angle(FR2);
  
  robot.theta[BL][0] = get_motor_angle(BL0);
  robot.theta[BL][1] = get_motor_angle(BL1);
  robot.theta[BL][2] = get_motor_angle(BL2);
  
  robot.theta[BR][0] = get_motor_angle(BR0);
  robot.theta[BR][1] = get_motor_angle(BR1);
  robot.theta[BR][2] = get_motor_angle(BR2);
}
/*
相位切换,通过时间和足底传感器确定支撑对角腿和摆动对角腿
*/

void phase_swap()
{
  if(robot.t > 0.75*robot.Tf)
  {
    if(robot.Fst == FL)  //支撑腿是FL,BR
    {
      if((robot.is_touching[FR])&&(robot.is_touching[BL]))
      {
        robot.Fst = FR;
        robot.Fsw = FL;
        robot.Bst = BL;
        robot.Bsw = BR;
        robot.t = 0;      //相位切换时候时间归零
      }
    }
    else if(robot.Fst == FR)  //支撑腿是FR,BL
    {
      if((robot.is_touching[FL])&&(robot.is_touching[BR]))
      {
        robot.Fst = FL;
        robot.Fsw = FR;
        robot.Bst = BR;
        robot.Bsw = BL;
        robot.t = 0;     //相位切换时候时间归零
      }
    }
  }
}
/*
运动学正解
*/
void forwardKinematics()
{
  double a0        = robot.a0;
  double a1        = robot.a1;
  double a2        = robot.a2;
  double h         = robot.h;
  double l         = robot.l;
  double w         = robot.w;
  double delta[4]  = { 1, 1,-1,-1 };
  double lambda[4] = { 1,-1, 1,-1 };
  
  //足底位置
  for(legNameTypeDef legIdx = FL; legIdx <= BR; legIdx++)
  {
    double s0  = sin( robot.theta[legIdx][0]/180.0*PI );
    double s1  = sin( robot.theta[legIdx][1]/180.0*PI );
    double c0  = cos( robot.theta[legIdx][0]/180.0*PI );
    double c1  = cos( robot.theta[legIdx][1]/180.0*PI );
    double s12 = sin( robot.theta[legIdx][1]/180.0*PI + robot.theta[legIdx][2]/180.0*PI);
    double c12 = cos( robot.theta[legIdx][1]/180.0*PI + robot.theta[legIdx][2]/180.0*PI);
    
    robot.toe[legIdx].x = -a1*s1 - a2*s12 + delta[legIdx]*l;
    robot.toe[legIdx].y = a0*s0 + a1*s0*c1 + a2*s0*c12 + lambda[legIdx]*w;
    robot.toe[legIdx].z = -a0*c0 - a1*c0*c1 - a2*c0*c12 - h;
  }
  
  //足底速度
  static vect3TypeDef pre_toe[4];
  static bool first_run_flag = true;
  if(first_run_flag == true)
  {
    first_run_flag = false;
  }
  else
  {
    for(legNameTypeDef legIdx = FL; legIdx <= BR; legIdx++)
    {
      robot.dtoe[legIdx].x = (robot.toe[legIdx].x - pre_toe[legIdx].x)/(0.001*(double)TIME_STEP);
      robot.dtoe[legIdx].y = (robot.toe[legIdx].y - pre_toe[legIdx].y)/(0.001*(double)TIME_STEP);
      robot.dtoe[legIdx].z = (robot.toe[legIdx].z - pre_toe[legIdx].z)/(0.001*(double)TIME_STEP);
    }
  }
  for(legNameTypeDef legIdx = FL; legIdx <= BR; legIdx++)
  {
    pre_toe[legIdx] = robot.toe[legIdx];
  }
}
/*
机器人水平速度估计
*/
void estimate_vx_vy_wz()
{
  static vect3TypeDef pre_F_toe, pre_B_toe;
         vect3TypeDef     F_toe,     B_toe;
  static double pre_alpha;  //角度制
         double     alpha;  //角度制
  
  if(robot.Fst == FL)
  {
    F_toe = robot.toe[FL];
    B_toe = robot.toe[BR];
  }
  else if(robot.Fst == FR)
  {
    F_toe = robot.toe[FR];
    B_toe = robot.toe[BL];
  }
  alpha = atan((F_toe.y - B_toe.y)/(F_toe.x - B_toe.x))*180.0/PI;  //注意，分母可能为0
  
  if(robot.t != 0)
  {
    robot.vx = -((F_toe.x - pre_F_toe.x + B_toe.x - pre_B_toe.x)/2.0)/(0.001*(double)TIME_STEP);
    robot.vy = -((F_toe.y - pre_F_toe.y + B_toe.y - pre_B_toe.y)/2.0)/(0.001*(double)TIME_STEP);
    robot.wz = -(alpha - pre_alpha)/(0.001*(double)TIME_STEP);
  }
  
  pre_F_toe = F_toe;
  pre_B_toe = B_toe;
  pre_alpha = alpha;
  
  //滤波
 static double pre_vx = 0;
 static double pre_vy = 0;
 static double pre_wz = 0;
 
 double k = min(1, robot.t/robot.Ts);  //新数据的权重，t越大，新数据权重越大，最大为1
 
 robot.vx = pre_vx*(1 - k) + robot.vx*k;
 robot.vy = pre_vy*(1 - k) + robot.vy*k;
 robot.wz = pre_wz*(1 - k) + robot.wz*k;
 
 pre_vx = robot.vx;
 pre_vy = robot.vy;
 pre_wz = robot.wz;
}
/*
更新摆动足的起始位置和起始速度
*/
void update_toe0_dtoe0()
{
  static vect3TypeDef pre_Fst_toe = {0,0,0};
  static vect3TypeDef pre_Bst_toe = {0,0,0};
  static vect3TypeDef Fst_dtoe    = {0,0,0};
  static vect3TypeDef Bst_dtoe    = {0,0,0};
  if(robot.t == 0) //刚刚切换过对角腿，记录此时的摆动足即可
  {
    //记录初始位置
    robot.Ftoe0 = robot.toe[robot.Fsw];
    robot.Btoe0 = robot.toe[robot.Bsw];
    //将初始位置转换到单腿坐标系
    if(robot.Fst == FL)
    {
      robot.Ftoe0.x -= robot.l;
      robot.Ftoe0.y += robot.w;  //FR
      robot.Btoe0.x += robot.l;
      robot.Btoe0.y -= robot.w;  //BL
    }
    else if(robot.Fst == FR)
    {
      robot.Ftoe0.x -= robot.l;
      robot.Ftoe0.y -= robot.w;  //FL
      robot.Btoe0.x += robot.l;
      robot.Btoe0.y += robot.w;  //BR
    }
   //记录初始速度
    robot.Fdtoe0 = Fst_dtoe;
    robot.Bdtoe0 = Bst_dtoe;
  }
  else
  {
    Fst_dtoe.x = (robot.toe[robot.Fst].x - pre_Fst_toe.x)/(0.001*((double)TIME_STEP));
    Fst_dtoe.y = (robot.toe[robot.Fst].y - pre_Fst_toe.y)/(0.001*((double)TIME_STEP));
    Fst_dtoe.z = (robot.toe[robot.Fst].z - pre_Fst_toe.z)/(0.001*((double)TIME_STEP));
    
    Bst_dtoe.x = (robot.toe[robot.Bst].x - pre_Bst_toe.x)/(0.001*((double)TIME_STEP));
    Bst_dtoe.y = (robot.toe[robot.Bst].y - pre_Bst_toe.y)/(0.001*((double)TIME_STEP));
    Bst_dtoe.z = (robot.toe[robot.Bst].z - pre_Bst_toe.z)/(0.001*((double)TIME_STEP));
  }
  pre_Fst_toe = robot.toe[robot.Fst];
  pre_Bst_toe = robot.toe[robot.Bst];
}
/*
足底err积分
*/
void update_toe_err_sum()
{
  if(robot.t == 0)
  {
    robot.tes[robot.Fsw] += (0 - robot.Ftoe0.y);
    robot.tes[robot.Bsw] += (0 - robot.Btoe0.y);
  }
}
/*
足底轨迹
*/
void curve(vect3TypeDef* ft, vect3TypeDef* dft, double xT, double yT, vect3TypeDef toe0, vect3TypeDef dtoe0)
{
  double t   = robot.t;
  double x0  = toe0.x;
  double y0  = toe0.y;
  double z0  = toe0.z;
  double dx0 = dtoe0.x;
  double dy0 = dtoe0.y;
//  double dz0 = dtoe0.z;
  double Tf  = robot.Tf;
  double zh  = robot.zh;
  //x,y
  if(t < Tf/4.0)
  {
    ft->x  = -4*dx0*t*t/Tf + dx0*t + x0;
    ft->y  = -4*dy0*t*t/Tf + dy0*t + y0;
    
    dft->x = -8*dx0*t/Tf + dx0;
    dft->y = -8*dy0*t/Tf + dy0;
  }
  else if((t >= Tf/4.0)&&(t < 3.0*Tf/4.0))
  {
    ft->x  = ( -4*Tf*dx0 - 16*xT + 16*x0)*t*t*t/(Tf*Tf*Tf) + 
             (  7*Tf*dx0 + 24*xT - 24*x0)*t*t/(Tf*Tf) + 
             (-15*Tf*dx0 - 36*xT + 36*x0)*t/(4*Tf) + 
             (  9*Tf*dx0 + 16*xT)/16;
            
    ft->y  = ( -4*Tf*dy0 - 16*yT + 16*y0)*t*t*t/(Tf*Tf*Tf) + 
             (  7*Tf*dy0 + 24*yT - 24*y0)*t*t/(Tf*Tf) + 
             (-15*Tf*dy0 - 36*yT + 36*y0)*t/(4*Tf) + 
             (  9*Tf*dy0 + 16*yT)/16;
            
    dft->x = ( -4*Tf*dx0 - 16*xT + 16*x0)*3*t*t/(Tf*Tf*Tf) + 
             (  7*Tf*dx0 + 24*xT - 24*x0)*2*t/(Tf*Tf) + 
             (-15*Tf*dx0 - 36*xT + 36*x0)/(4*Tf);
             
    dft->y = ( -4*Tf*dy0 - 16*yT + 16*y0)*3*t*t/(Tf*Tf*Tf) + 
             (  7*Tf*dy0 + 24*yT - 24*y0)*2*t/(Tf*Tf) + 
             (-15*Tf*dy0 - 36*yT + 36*y0)/(4*Tf);
  }
  else
  {
    ft->x  = xT;
    ft->y  = yT;
    dft->x = 0;
    dft->y = 0;
  }
  //z
  if(t < Tf/2.0)
  {
    ft->z  = 16*(z0 - zh)*t*t*t/(Tf*Tf*Tf) + 12*(zh - z0)*t*t/(Tf*Tf) + z0;
    dft->z = 16*(z0 - zh)*3*t*t/(Tf*Tf*Tf) + 12*(zh - z0)*2*t/(Tf*Tf);
  }
  else
  {
    ft->z  = 4*(z0 - zh)*t*t/(Tf*Tf) - 4*(z0 - zh)*t/Tf + z0;
    dft->z = 4*(z0 - zh)*2*t/(Tf*Tf) - 4*(z0 - zh)/Tf;
  }
}
/*
创建Q的逆矩阵
*/
void create_invQ(matTypeDef* invQ, vect3TypeDef stF_toe, vect3TypeDef stB_toe)
{
  double xF = stF_toe.x;
  double yF = stF_toe.y;
  double zF = stF_toe.z;
  double xH = stB_toe.x;
  double yH = stB_toe.y;
  double zH = stB_toe.z;
  // o表示加, _表示减
  double xF_xH = xF - xH;
  double xFoxH = xF + xH;
  double xH_xF = xH - xF;
//  double xHoxF = xH + xF;
  double yF_yH = yF - yH;
//  double yFoyH = yF + yH;
  double zF_zH = zF - zH;
  double zFozH = zF + zH;
  double zH_zF = zH - zF;
//  double zHozF = zH + zF;
  
  double xFzH_xHzF = xF*zH - xH*zF;
  double xHzF_xFzH = xH*zF - xF*zH;
  double xFyH_xHyF = xF*yH - xH*yF;
  double xHyF_xFyH = xH*yF - xF*yH;
  double yFzH_yHzF = yF*zH - yH*zF;
  double yHzF_yFzH = yH*zF - yF*zH;
  
  invQ->data[0][0] = (xF_xH*yH*zF-(xFoxH*yF-2*xF*yH)*zH)/(2*yF_yH*xHzF_xFzH);
  invQ->data[0][1] = xFoxH*xFyH_xHyF/(2*yF_yH*xFzH_xHzF);
  invQ->data[0][2] = xF_xH*xFoxH/(2*yF_yH*xHzF_xFzH);
  invQ->data[0][3] = xFoxH/(2*xHzF_xFzH);
  invQ->data[0][4] = xF_xH*zFozH/(2*yF_yH*xHzF_xFzH);
  invQ->data[0][5] = 0;
  
  invQ->data[1][0] = yHzF_yFzH/(2*xHzF_xFzH);
  invQ->data[1][1] = xHyF_xFyH/(2*xHzF_xFzH);
  invQ->data[1][2] = xF_xH/(2*xHzF_xFzH);
  invQ->data[1][3] = yF_yH/(2*xHzF_xFzH);
  invQ->data[1][4] = zF_zH/(2*xHzF_xFzH);
  invQ->data[1][5] = 0;
  
  invQ->data[2][0] = zFozH*yHzF_yFzH/(2*yF_yH*xHzF_xFzH);
  invQ->data[2][1] = (xF*yH*zH_zF+xH*(yF*zFozH-2*yH*zF))/(2*yF_yH*xHzF_xFzH);
  invQ->data[2][2] = xFoxH*zF_zH/(2*yF_yH*xHzF_xFzH);
  invQ->data[2][3] = zFozH/(2*xHzF_xFzH);
  invQ->data[2][4] = zF_zH*zFozH/(2*yF_yH*xHzF_xFzH);
  invQ->data[2][5] = 0;
  
  invQ->data[3][0] = ((2*xH*yF-xFoxH*yH)*zF+xH_xF*yF*zH)/(2*yF_yH*xHzF_xFzH);
  invQ->data[3][1] = xFoxH*xFyH_xHyF/(2*yF_yH*xHzF_xFzH);
  invQ->data[3][2] = xF_xH*xFoxH/(2*yF_yH*xFzH_xHzF);
  invQ->data[3][3] = xFoxH/(2*xFzH_xHzF);
  invQ->data[3][4] = -xF_xH*zFozH/(2*yF_yH*xHzF_xFzH);
  invQ->data[3][5] = 0;
  
  invQ->data[4][0] = yHzF_yFzH/(2*xHzF_xFzH);
  invQ->data[4][1] = xHyF_xFyH/(2*xHzF_xFzH);
  invQ->data[4][2] = xF_xH/(2*xHzF_xFzH);
  invQ->data[4][3] = yF_yH/(2*xHzF_xFzH);
  invQ->data[4][4] = zF_zH/(2*xHzF_xFzH);
  invQ->data[4][5] = 0;

  invQ->data[5][0] = zFozH*yFzH_yHzF/(2*yF_yH*xHzF_xFzH);
  invQ->data[5][1] = (xH*yF*zF_zH+xF*(yH*zFozH-2*yF*zH))/(2*yF_yH*xHzF_xFzH);
  invQ->data[5][2] = xFoxH*zF_zH/(2*yF_yH*xHzF_xFzH);
  invQ->data[5][3] = zFozH/(2*xFzH_xHzF);
  invQ->data[5][4] = (zH*zH-zF*zF)/(2*yF_yH*xHzF_xFzH);
  invQ->data[5][5] = 0;
}
/*
创建力雅可比
*/
void create_transJ(matTypeDef* transJ, legNameTypeDef Fleg, legNameTypeDef Bleg)
{
  double a0 = robot.a0;
  double a1 = robot.a1;
  double a2 = robot.a2;
  
  double Fc0  = cos(robot.theta[Fleg][0]/180.0*PI);
  double Bc0  = cos(robot.theta[Bleg][0]/180.0*PI);
  double Fs0  = sin(robot.theta[Fleg][0]/180.0*PI);
  double Bs0  = sin(robot.theta[Bleg][0]/180.0*PI);
  double Fc1  = cos(robot.theta[Fleg][1]/180.0*PI);
  double Bc1  = cos(robot.theta[Bleg][1]/180.0*PI);
  double Fs1  = sin(robot.theta[Fleg][1]/180.0*PI);
  double Bs1  = sin(robot.theta[Bleg][1]/180.0*PI);
  double Fc12 = cos(robot.theta[Fleg][1]/180.0*PI + robot.theta[Fleg][2]/180.0*PI);
  double Bc12 = cos(robot.theta[Bleg][1]/180.0*PI + robot.theta[Bleg][2]/180.0*PI);
  double Fs12 = sin(robot.theta[Fleg][1]/180.0*PI + robot.theta[Fleg][2]/180.0*PI);
  double Bs12 = sin(robot.theta[Bleg][1]/180.0*PI + robot.theta[Bleg][2]/180.0*PI);
  
  transJ->data[0][0] = 0;
  transJ->data[0][1] = Fc0*(a0 + a1*Fc1 + a2*Fc12);
  transJ->data[0][2] = Fs0*(a0 + a1*Fc1 + a2*Fc12);
  transJ->data[0][3] = 0;
  transJ->data[0][4] = 0;
  transJ->data[0][5] = 0;
  
  transJ->data[1][0] = -a1*Fc1 - a2*Fc12;
  transJ->data[1][1] = -Fs0*(a1*Fs1 + a2*Fs12);
  transJ->data[1][2] =  Fc0*(a1*Fs1 + a2*Fs12);
  transJ->data[1][3] = 0;
  transJ->data[1][4] = 0;
  transJ->data[1][5] = 0;
  
  transJ->data[2][0] = -a2*Fc12;
  transJ->data[2][1] = -a2*Fs0*Fs12;
  transJ->data[2][2] =  a2*Fc0*Fs12;
  transJ->data[2][3] = 0;
  transJ->data[2][4] = 0;
  transJ->data[2][5] = 0;
  
  transJ->data[3][0] = 0;
  transJ->data[3][1] = 0;
  transJ->data[3][2] = 0;
  transJ->data[3][3] = 0;
  transJ->data[3][4] = Bc0*(a0 + a1*Bc1 + a2*Bc12);
  transJ->data[3][5] = Bs0*(a0 + a1*Bc1 + a2*Bc12);

  transJ->data[4][0] = 0;
  transJ->data[4][1] = 0;
  transJ->data[4][2] = 0;
  transJ->data[4][3] = -a1*Bc1 - a2*Bc12;
  transJ->data[4][4] = -Bs0*(a1*Bs1 + a2*Bs12);
  transJ->data[4][5] =  Bc0*(a1*Bs1 + a2*Bs12);

  transJ->data[5][0] = 0;
  transJ->data[5][1] = 0;
  transJ->data[5][2] = 0;
  transJ->data[5][3] = -a2*Bc12;
  transJ->data[5][4] = -a2*Bs0*Bs12;
  transJ->data[5][5] =  a2*Bc0*Bs12;
}